% Florian Meyer, 22/04/15.

function [posteriorReweigthed, rmse ] = spawnFilter( agentPos, measurements, prior, anchorPos, inputs, parameter, n)
[agentNum, ~] = size(agentPos);
[anchorNum, ~] = size(anchorPos);

measurementsAgentsAnchors = measurements{1};
measurementsAgentsAgents = measurements{2};

%initialize parameters
measurementVariance = parameter.measurementVariance;
dynamicsVariance = parameter.dynamicsVariance;
particlesNum = parameter.oversampling*parameter.particlesNum;
kernelSamplingAt = parameter.kernelSamplingAt;

%initialize variouse vectors
estimate = zeros(agentNum,2);
rmse = zeros(agentNum,1);
agentUncertainties = zeros(agentNum,1);
smartResampling = zeros(agentNum,1);
proposal = zeros(particlesNum,2,agentNum);
posteriorReweigthed = zeros(particlesNum,2,agentNum);

%use the incoming message from the anchor as proposal distribution at time first time step (prior is too uninformative)
anchorProposal = 0;
if(n == 1)
    anchorProposal = 1;
end

for agent = 1:agentNum   
    if(anchorProposal)
        %use the first incoming message from the anchor as proposal distribution for the agent state
        measurementVarianceTmp = measurementVariance;
        if(parameter.pathloss)
            if(measurementsAgentsAnchors(agent,1) > parameter.distZero)
                measurementVarianceTmp = ((measurementsAgentsAnchors(agent,1)/parameter.distZero - 1)^parameter.kappa + 1)*measurementVariance;
            else
                measurementVarianceTmp = measurementVariance;
            end
        end
        angle = 2 * pi * rand(particlesNum,1);
        noise = sqrt(measurementVarianceTmp) * randn(particlesNum,1);
        proposal(:,:,agent) = zeros(particlesNum,2);
        proposal(:,1,agent) = anchorPos(1,1) + ( measurementsAgentsAnchors(agent,1) + noise ) .* cos(angle);
        proposal(:,2,agent) = anchorPos(1,2) + ( measurementsAgentsAnchors(agent,1) + noise ) .* sin(angle);
    else
        %use the predicted posterior as proposal distribution (this involves the control input)
        proposal(:,:,agent) = prior(:,:,agent) + repmat(inputs(agent,:),particlesNum,1) + sqrt(dynamicsVariance)*randn(particlesNum,2);
    end
    
    %determine if smart resampling is performed
    agentUncertainties(agent) = mean(diag(cov(proposal(:,:,agent))));    
    if(mod(n,kernelSamplingAt/4) == 0 && n ~= 0 && agentUncertainties(agent) > 100*parameter.censoringVar)
        smartResampling(agent) = true;
    elseif(mod(n,kernelSamplingAt/2) == 0 && n ~= 0 && agentUncertainties(agent) > 10*parameter.censoringVar)
        smartResampling(agent) = true;
    elseif(mod(n,kernelSamplingAt) == 0 && n ~= 0)
        smartResampling(agent) = true;
    end   
end

%evaluate messages related to pairwise measurements with the other agents at the particles
for agent = 1:agentNum    
    
    %initilize weights vectors
    weights = ones(particlesNum,1);
    weightsPathloss = ones(particlesNum,1);    
    
    %if the first incoming message from the anchor was used as proposal distribution, reduce the number of messages to be evaluated by one
    numMessages = anchorNum - anchorProposal;
    
    if (numMessages > 0)        
        %evaluate messages related to pairwise measurements with anchors
        for anchor = (1+anchorProposal):anchorNum
            tmpDist = sqrt( (proposal(:,1,agent)-anchorPos(anchor,1)).^2 + (proposal(:,2,agent)-anchorPos(anchor,2)).^2 );
            measurementVarianceTmp = measurementVariance;
            if(parameter.pathloss)
                idx1 = tmpDist > parameter.distZero;
                idx2 = tmpDist <= parameter.distZero;
                measurementVarianceTmp = zeros(particlesNum,1);
                measurementVarianceTmp(idx1,1) = ((tmpDist(idx1)/parameter.distZero - 1).^parameter.kappa + 1)*measurementVariance;
                measurementVarianceTmp(idx2,1) = measurementVariance;
                weightsPathloss = weightsPathloss./(sqrt(measurementVarianceTmp));
            end
            tmpWeights = -1./(2*measurementVarianceTmp) .* (measurementsAgentsAnchors(agent,anchor) - tmpDist).^2;
            tmpWeights = tmpWeights  - max(tmpWeights);
            tmpWeights = exp(tmpWeights);
            weights = weights.*tmpWeights;
            weights = 1/sum(weights)*weights;
        end        
        
        %evaluate messages related to pairwise measurements with other mobile agents
        for partnerAgent = 1:agentNum
            if (agent == partnerAgent ||  agentUncertainties(partnerAgent) > parameter.censoringVar)
                continue;
            end
            tmpDist = sqrt( (proposal(:,1,agent)-proposal(:,1,partnerAgent)).^2 + (proposal(:,2,agent)-proposal(:,2,partnerAgent)).^2 );
            if(parameter.pathloss)
                idx1 = tmpDist > parameter.distZero;
                idx2 = tmpDist <= parameter.distZero;
                measurementVarianceTmp = zeros(particlesNum,1);
                measurementVarianceTmp(idx1,1) = ((tmpDist(idx1)/parameter.distZero - 1).^parameter.kappa + 1)*measurementVariance;
                measurementVarianceTmp(idx2,1) = measurementVariance;
                weightsPathloss = weightsPathloss./(sqrt(measurementVarianceTmp));
            end
            tmpWeights = -1./(2*measurementVariance) .* (measurementsAgentsAgents(agent,partnerAgent) - tmpDist).^2;
            tmpWeights = tmpWeights  - max(tmpWeights);
            tmpWeights = exp(tmpWeights);
            weights = weights.*tmpWeights;
            weights = 1/sum(weights)*weights;
        end        
        weights = weightsPathloss.*weights;
        weights = 1/sum(weights)*weights;
        
        %perform resampling
        posteriorReweigthed(:,:,agent) = kernelDensityResampling(weights, proposal(:,:,agent), particlesNum, measurementVariance, smartResampling(agent));
    else
        %if the number of messages to be evaluated is zero the proposal is equivalent to the belief
        posteriorReweigthed(:,:,agent) = proposal(:,:,agent);
    end
    
    %calculate estimate and rmse
    estimate(agent,:) = mean(posteriorReweigthed(:,:,agent));
    rmse(agent,:) = sqrt(sum((estimate(agent,:) - agentPos(agent,:)).^2));
end
end